import { Message } from 'element-ui'
import { DATA } from './data'
import { subButtery } from './methods'
import store from '../store/index'
//参数
var PARAMS = PARAMS || {
	DefaultPose: {
    	position: {
    			x: 0,
    			y: 0,
    			z: 0
    	},
    	orientation: {
    			x: 0,
    			y: 0,
    			z: 0,
    			w: 1
    	}
    },
    // cmd_vel for docking
    CmdVel: 0.05,
    // backward moving distance before navigation 
    SailDis: 0.30,
    // distance between dock and docking begin waypoint
    DockingBeginDis: 0.35,
    // threshold for low power warning
	PowerWarnThreshold: 0.2,
	
	ChargeStatus: {
    	uncontact: "0",
    	charging: "1"
	},
	
    NavCtrl: {
    	stop: 0,
    	start: 1,
    	pause: 2
    },

	//小车导航状态
    NavCtrlStatus: {
    	error: -1,
    	idling: 0,
    	running: 1,
    	paused: 2,
    	completed: 3,
    	cancelled: 4,
    	sub_cancelled: 5
    },

    NetworkMode: {
    	ap: 'ap',
    	wifi: 'wifi'
    },
    
    RobotStatus: {
    	MappingStatus: 'mappingStatus',
    },

    DiagnosticsLevel: {
    	Warn: 1,
    	Error: 2,
    	Stale: 3
    },
};


//定位
var NAV = NAV || {
	CmdEnum: {
        Navigation: "navigation",
        Gmapping: "gmapping",
        Cancel: "cancel",
        Converting: "converting",
        GamppingPose: "gmapping_pose",
        SaveMap: "save_map",
        SaveMapEdit: "save_map_edit",
        SaveAsMap: "save_as_map",
        SaveAsMapEdit: "save_as_map_edit",
        LoadMap: "load_map",
        LoadMapEdit: "load_map_edit",
        Userauth: "user_auth",
        MapSelect: "dbparam-select",  //查询地图
        MapDelete: "dbparam-delete",  //删除地图
        MapUpdate: "dbparam-update", // 切换地图
        MapInsert: "dbparam-insert", // 添加地图
        Update: "update",
        RoslogDelete: "roslog-delete",
        RoslogSelect: "roslog-select",
        Version: "version",
    },

	//地图是新建、导航、切换
    RosMode: {
    	Gmapping: 'gmapping',//建图状态
    	Navigation: 'navigation',//导航状态
    	Converting: 'converting'//切换地图状态
    },

    WaypointMode: {
    	Map: 'map',
    	Timer: 'timer',
    	Puber: 'publisher',
    	Suber: 'subscriber',
    	Pubsuber: 'pubsuber',
    	Looper: 'looper',
    	CmdVelSetSub: 'cmd_vel_set_sub',
    	CmdVel: 'cmd_vel',
    	Shell: 'shell',
    	SoundPlay: 'sound_play',
    	InitialPose: 'initial_pose',
    	ScanMaker: 'scan_marker',
    	ShellString: 'shell_string',
    	Pallet: 'pallet'
    },

    SoundPlayMode: {
    	Stop: 'STOP',
    	Start: 'START',
    	Once: 'ONCE'
    },

    WaypointPrefix: {
    	goal: 'goal',
    	timer: 'timer',
    	publisher: 'pub',
    	subscriber: 'sub',
    	looper: 'loop',
    	pubsuber: 'pubsuber',
    	cmd_vel_set_sub: 'velSet',
    	cmd_vel: 'vel',
    	shell: 'shell',
    	sound_play: 'sound',
    	initial_pose: 'pose',
    	scan_marker: 'scanMarker',
    	shell_string: 'shellStr',
    	pallet: 'pallet'
    },

    ManualCtrlVel: {
    	linear: 0.4,
    	angular: 0.4
    },
	
	/**
	 * 初始化ros
	 * **/
	init: (url) => {
		NAV.ros = new ROSLIB.Ros();
		NAV.ros.connect(url);//链接
		//链接到websocket服务器
		NAV.ros.on('connection', () => {
			console.log(`[INFO]Connected to rosbridge ${url}.`);
			DATA.ros = NAV.ros;
			DATA.loading = false;
		});
		//关闭websocket服务器
		NAV.ros.on('close', () => {
			console.log(`[INFO]Rosbridge server ${url} closed.`);
			DATA.loading = false;
			Message({
				showClose: true,
				message: 'Rosbridge连接失败',
				type: 'error',
				duration: 0
			});
		});
		//关闭websocket服务器出错
		NAV.ros.on('error', () => {
			DATA.loading = false;
			Message({
				showClose: true,
				message: 'Rosbridge连接失败',
				type: 'error',
				duration: 0
			});
			console.error(`[ERROR]Connection error.`);
		});
	},

	/**
	 * 订阅地图信息,再发送base64格式节点，接受到时base64的图片
	 * 实时获取地图数据等
	 * 再订阅站点
	 * **/
	dispMapAndWps: (useBase64) => {
		var mapTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/map'),
			messageType: 'nav_msgs/OccupancyGrid'
		});

		if (useBase64){
			DATA.useBase64 = true;
			mapTopic = new ROSLIB.Topic({
				ros: NAV.ros,
				name: withNs('/map_stream'),
				messageType: 'scheduling_msgs/MapStream'
			});
		}
		DATA.topic['mapTopic'] = mapTopic;
		mapTopic.subscribe((message) => {
			DATA.mapMsg = message;//地图信息赋值，包含宽高缩放等
			NAV.subStations();
		});
	},

	/**
	 * 订阅站点
	 * 再渲染站点
	 * **/
	subStations:() => {
		var wpTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/waypoints'),
			messageType: 'yocs_msgs/WaypointList'
		});
		DATA.topic['waypointsTopic'] = wpTopic;
		wpTopic.subscribe((message) => {
			//获取到站点信息之后赋给waypointsMsg，渲染站点，添加事件
			DATA.waypointsMsg = message;
		});
	},

	/**
	 * 订阅轨迹
	 * 轨迹列表
	 * **/
	dispTrajectories: () => {
		var trajTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/trajectories'),
			messageType: 'yocs_msgs/TrajectoryList'
		});
		DATA.topic['trajectoriesTopic'] = trajTopic;
		trajTopic.subscribe((message) => {
			console.log('[INFO]Got trajectories.');
			DATA.trajectoriesMsg = message;
		});
	},

	/**
	 *订阅 获取机器人姿态和位置
	 * **/
	dispRobot: () => {
		var robotPoseTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/amcl_pose'),
			messageType: 'geometry_msgs/PoseWithCovarianceStamped'
		});
		DATA.topic['robotPoseTopic'] = robotPoseTopic;
		robotPoseTopic.subscribe((message) => {
			DATA.robotPoseMsg = message;
		});
	},

	/**
	 * 订阅ros所有状态
	 * 地图状态、充电状态等
	 * auto_charge：0非充电，1在充电
	 * cartographer_node： 0非建图，1建图
	 * **/
	subRosStatus:() => {
		let topic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/task_state'),
			messageType: 'diagnostic_msgs/DiagnosticStatus'
		});
		DATA.topic['rosStatusTopic'] = topic;
		topic.subscribe((msg) => {
			msg.values.map(function(e,i){
				if(e.key === 'auto_charge'){
					DATA.chargeStatus = e.value;
					switch(e.value){
						case '0':
						store.commit("switchBatteryStatus", null)
						break;

						case '1':
						store.commit("switchBatteryStatus", 'text')
						break;
					}
				}else if(e.key === 'cartographer_node'){
					if(e.value === "0"){
						DATA.rosMode = "navigation"
					}else if(e.value === "1"){
						DATA.rosMode = "gmapping"
					}else{

					}
				}
			})
			//msg.values.[{auto_charge:"0"},{cartographer_node:"0"},{amcl:"1"},{move_base:"1"},{shelf_detector:"0"}]
		})
	},

	/**
	 * 订阅小车导航状态
	 * message： 0,4非执行任务，否则位执行任务
	 * **/
	subNavCtrlStatus: () => {
		var navCtrlStatusTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: '/nav_ctrl_status',
			messageType: 'yocs_msgs/NavigationControlStatus'
		});
		DATA.topic['navCtrlStatusTopic'] = navCtrlStatusTopic;
		navCtrlStatusTopic.subscribe((message) => {
			DATA.navCtrlStatus = message;
		});
	},

	/**
	 * 导航到指定站点
	 * name:站点名称
	 * DATA.navCtrlStatus.status :0或4是空闲状态
	 * control：0是停止，1是启动
	 * **/
	navCtrl: (name, control, ...rest) => {
		var allowInterrupt = false;
		if (rest[0] === true){
			allowInterrupt = true;
		}
		console.log(`[INFO] navstatus ${DATA.navCtrlStatus.status}`)
		if (!allowInterrupt){
			if (DATA.navCtrlStatus.status !== PARAMS.NavCtrlStatus.idling && DATA.navCtrlStatus.status !== PARAMS.NavCtrlStatus.cancelled){
				if (control === PARAMS.NavCtrl.start){//=1
					Message({
						showClose: true,
						message: '正在执行其他任务，当前命令将被忽略',
						type: 'error',
						duration: 3000
					});
					return;
				}
			}
		}
		// verifyChargeStatus();
		var navCtrlTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/nav_ctrl'),
			messageType: 'yocs_msgs/NavigationControl'
		});
		DATA.topic['navCtrlTopic'] = navCtrlTopic;
		var msg = new ROSLIB.Message({
            goal_name: name,
            control: control
		});
		console.log(`[INFO]pub ${name}, ${control}`);
		navCtrlTopic.publish(msg);
	},



	/**
	 * 提交添加站点
	 * **/
	addWaypoint: (options) => {
		var addWaypointTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/waypoint_add'),
			messageType: 'yocs_msgs/Waypoint'
		});
		DATA.topic['addWaypointTopic'] = addWaypointTopic;
		let frameIdStr = {
			close_enough: options.closeEnough,
			failure_mode: options.stationMode,
			frame_id: options.frameId,
			goal_timeout: options.timeout,
			type: options.stationType
		}
		var msg = new ROSLIB.Message({
			header: {
				frame_id: JSON.stringify(frameIdStr)
			},
			name: options.stationName,
			pose: options.pose,
			close_enough: 0.0,
			goal_timeout: 0.0,
			failure_mode: options.stationMode,
		});
		addWaypointTopic.publish(msg);
		console.log('[INFO]waypoint added');
		Message({
			showClose: true,
			message: '站点添加成功',
			type: 'success',
			duration: 3000
		});
	},

	/**
	 * 删除站点
	 * **/
	delWaypoint: (name) => {
		var delWaypointTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/waypoint_remove'),
			messageType: 'yocs_msgs/Waypoint'
		});
		DATA.topic['delWaypointTopic'] = delWaypointTopic;
		var waypoint = getWaypointByName(name);//站点对象
		var msg = new ROSLIB.Message(waypoint);
		console.log(`delete Waypoint Name:${waypoint}`)
		delWaypointTopic.publish(msg);
		Message({
			showClose: true,
			message: '站点删除成功',
			type: 'success',
			duration: 3000
		});
	},

	/**
	 * 提交添加轨迹话题
	 * name:轨迹名称，station
	 * **/
	addTrajectory: (infos) => {
		let stationsName = infos.stations;
		let addTrajectoryTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/trajectory_add'),
			messageType: 'yocs_msgs/Trajectory'
		});
		DATA.topic['addTrajectoryTopic'] = addTrajectoryTopic;
		let waypoints = [];
		for (let i = 0; i < stationsName.length; i++){
			let selectedWpName = stationsName[i];
			waypoints.push(getWaypointByName(selectedWpName));
		}
		let msg = new ROSLIB.Message({
			name: infos.name,
			waypoints: waypoints
		});
		addTrajectoryTopic.publish(msg);
		Message({
			showClose: true,
			message: '轨迹添加成功',
			type: 'success',
			duration: 3000
		});
	},

	/**
	 * 删除轨迹
	 * **/
	delTrajectory: (name) => {
		var delTrajTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/trajectory_remove'),
			messageType: 'yocs_msgs/Trajectory'
		});
		DATA.topic['delTrajectoryTopic'] = delTrajTopic;
		var traj = getTrajectoryByName(name);
		var msg = new ROSLIB.Message(traj);
		console.log(`delete Trajectory Name:${traj}`)
		delTrajTopic.publish(msg);
		Message({
			showClose: true,
			message: '轨迹删除成功',
			type: 'success',
			duration: 3000
		});
	},

	/**
	 * 控制小车移动方向
	 * 0：停止
	 * **/
	manualCtrl: (vel) => {
		verifyChargeStatus();
		var linear = vel.linear || 0;
		var angular = vel.angular || 0;
		var cmdVelTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/cmd_vel'),
			messageType: 'geometry_msgs/Twist'
		});
		var msg = new ROSLIB.Message({
			linear: {
				x: linear,
				y: 0,
				z: 0
			},
			angular: {
				x: 0,
				y: 0,
				z: angular
			}
		});
		if (DATA.manualCtrlTimer){
			clearInterval(DATA.manualCtrlTimer);
			DATA.manualCtrlTimer = null;
		}
		if (linear === 0 && angular === 0){
			cmdVelTopic.publish(msg);
			return;
		}
		DATA.manualCtrlTimer = setInterval(function(){
			cmdVelTopic.publish(msg);
		}, 200);
	},


	//订阅全局路径
	dispGlobalPlan: () => {
		var globalPlanTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/move_base/NavfnROS/plan'),
			messageType: 'nav_msgs/Path'
		});
		DATA.topic['globalPlanTopic'] = globalPlanTopic;
		globalPlanTopic.subscribe((message) => {
			DATA.globalPlanMsg = message;
		});
	},

	//订阅局部路径
	dispLocalPlan: () => {
		var localPlanTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/move_base/TebLocalPlannerROS/local_plan'),
			messageType: 'nav_msgs/Path'
		});
		DATA.topic['localPlanTopic'] = localPlanTopic;
		localPlanTopic.subscribe((message) => {
			DATA.localPlanMsg = message;
		});
	},

	//订阅机器人轮廓
	dispFootprint: () => {
		var footprintTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/move_base/global_costmap/footprint'),
			messageType: 'geometry_msgs/PolygonStamped'
		});
		DATA.topic['footprintTopic'] = footprintTopic;
		footprintTopic.subscribe((message) => {
			DATA.footprintMsg = message;
		});
	},

	//订阅激光
	dispLaserScan: () => {
		var laserScanTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/scan_rectified'),
			messageType: 'sensor_msgs/LaserScan',
			throttle_rate: 200
		});
		DATA.topic['laserScanTopic'] = laserScanTopic;
		laserScanTopic.subscribe((message) => {
			DATA.laserScanMsg = message;
		});
	},



	/**
	 * 选择更新
	 * type:更新类型，info:对应的更新数据
	 * **/
	updateSystem: (type, info) => {
		var topic = '';
		switch (type){
			case 'online':
				NAV.pubCmdString(info);
				return;
			case 'offline':
				topic = '/shell_string';
				break;
			case 'dbparam':
				topic = '/system_shell/shell_string';
				break;
			case 'openssh':
				topic = '/shell_string';
				break;
			default:
				console.error(`[ERROR]unknown update mode: ${type}`);
				return;
		}
		var updateTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs(topic),
			messageType: 'std_msgs/String'
		});
		DATA.topic['updateTopic'] = updateTopic;
		var message = new ROSLIB.Message({
			data: info
		});
		updateTopic.publish(message);
	},




	/**
	 * 监听TFs
	 * 坐标转换，防止崩溃
	 * fixedFrame：固定针；angularThres：重新发布程序阈值
	 * transThres：从新发布程序转译阈值
	 * **/
	listenTf: (useTf2) => {
		var useTf2 = (useTf2 === undefined) ? true : useTf2;
		if (useTf2){
			//从ros中订阅TFS
			var tfClient = new ROSLIB.TFClient({
				ros: NAV.ros,
				fixedFrame: 'map',
				angularThres: 0.01,
				transThres: 0.01
			});
			tfClient.subscribe('odom', (tf) => {
				DATA.tfMsg['map2odom'] = {
					header: {
						stamp: null
					},
					transform: tf
				};
			});
			tfClient.subscribe('base_footprint', (tf) => {
				DATA.tfMsg['map2base_footprint'] = {
					header: {
						stamp: null
					},
					transform: tf
				};
			});
			tfClient.subscribe('base_laser', (tf) => {
				DATA.tfMsg['map2base_laser'] = {
					header: {
						stamp: null
					},
					transform: tf
				};
			});
			var tfClient2 = new ROSLIB.TFClient({
				ros: NAV.ros,
				fixedFrame: 'base_link',
				angularThres: 0.01,
				transThres: 0.01
			});
			tfClient2.subscribe('base_laser', (tf) => {
				DATA.tfMsg['base_link2base_laser'] = {
					header: {
						stamp: null
					},
					transform: tf
				};
			});
		}else{
			var tfTopic = new ROSLIB.Topic({
				ros: NAV.ros,
				name: '/tf',
				messageType: 'tf2_msgs/TFMessage',
				// throttle_rate: 50
			});
			tfTopic.subscribe((message) => {
				for (var i = 0; i < message.transforms.length; i++){
					var frame_id = message.transforms[i].header.frame_id;
					var child_frame_id = message.transforms[i].child_frame_id;
					if (frame_id.startsWith('/')){
						frame_id = frame_id.split('/')[1];
					}
					if (child_frame_id.startsWith('/')){
						child_frame_id = child_frame_id.split('/')[1];
					}
					var tfName = frame_id + '2' + child_frame_id; // eg: map2odom
					DATA.tfMsg[tfName] = message.transforms[i];
				}
			});
		}
	},

	/**
	 * 位置估计
	 * 提交最初位置
	 * **/
	sendInitialPose: (pose) => {
		var initialPoseTopic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/initialpose'),
			messageType: 'geometry_msgs/PoseWithCovarianceStamped'
		});
		var covariance = [];
		for (var i = 0; i < 36; i++){
			covariance[i] = 0;
		}
		covariance[0] = 0.25;
		covariance[7] = 0.25;
		covariance[35] = Math.pow((Math.PI/12), 2);
		var msg = new ROSLIB.Message({
			header: {
				frame_id: 'map'
			},
			pose: {
				pose: pose,
				covariance: covariance
			}
		});
		initialPoseTopic.publish(msg);
		console.log('[INFO]setting initial pose');
	},


	
	/**
	 * 地图操作提交
	 * 查询地图，切换地图，删除地图
	 * cmd：地图类型+名称
	 * **/
	pubCmdString: (cmd) => {
		var topic = new ROSLIB.Topic({
			ros: NAV.ros,
			name: withNs('/cmd_string'),
			messageType: 'std_msgs/String'
		});

		var msg = new ROSLIB.Message({
            data: cmd
		});
        topic.publish(msg);
	},
};


/**
 * 验证小车导航状态
 * 0和4是空置，取消状态
 * **/
export function verifyNavCtrlStatus(){
	if (DATA.navCtrlStatus.status !== PARAMS.NavCtrlStatus.idling && DATA.navCtrlStatus.status !== PARAMS.NavCtrlStatus.cancelled){
		Message({
			showClose: true,
			message: '正在执行其他任务，当前命令将被忽略',
			type: 'error',
			duration: 3000
		});
		return;
    }
}

/**
 * 验证充电状态
 * **/
 export function verifyChargeStatus(){
	 if(DATA.chargeStatus === PARAMS.ChargeStatus.charging){
		Message({
			showClose: true,
			message: '正在处于充电状态，当前命令将被忽略',
			type: 'error',
			duration: 3000
		});
		return;
	 }
 }

/**
 * 正则匹配特殊符号
 * */
export function checkStr(str) {
    var myReg = /^[^@\/\'\\\"\‘\’#$%&\^\*]+$/;
    return myReg.test(str);
}

//是否匹配版本
export function isEqual(actVal="", target){
    var val = actVal.substr(0, actVal.length-2);
    val = parseInt(val);
    return Math.abs(val-target)<2.0;
}

/**
 * 是否已指定的前缀"/"开始
 * **/
export function withNs(name){
	var name_ = name.startsWith('/') ? name : '/'+name;
	return name_;
};

/**
 * 通过名称找到对应站点数据
 * **/
export function getWaypointByName(name){
	for (var i = 0; i < DATA.waypointsMsg.waypoints.length; i++){
		var waypoint = DATA.waypointsMsg.waypoints[i];
		if (waypoint.name === name){
			return waypoint;
		}
	}
};

/**
 * 通过名称获取对应的轨迹数据
 * **/
export function getTrajectoryByName(name){
	var traj;
	for (var i = 0; i < DATA.trajectoriesMsg.trajectories.length; i++){
		traj = DATA.trajectoriesMsg.trajectories[i];
		if (traj.name === name){
			break;
		}
	}
	return traj;
};

/**
 * 站点名字是否重复
 * **/
export function isNameUsedWaypoints(name){
	if (DATA.waypointsMsg){
		for (var i = 0; i < DATA.waypointsMsg.waypoints.length; i++){
			if (DATA.waypointsMsg.waypoints[i].name === name){
				return true;
			}
		}	
	}
	return false;
};

/**
 * 轨迹名字是否重复
 * **/
export function isNameUsedtraject(name){
	if (DATA.trajectoriesMsg){
		for (var i = 0; i < DATA.trajectoriesMsg.trajectories.length; i++){
			if (DATA.trajectoriesMsg.trajectories[i].name === name){
				return true;
			}
		}
	}
	return false;
};
export { PARAMS, NAV }